%最大能力曲线计算
function State_max = speedPlanMP(lineStruct, trainStruct, qsStruct)
%% 设置求解参数
position_start = qsStruct.pos_start * 1000;
position_end = qsStruct.pos_end * 1000;
position_interval = qsStruct.step_x;
speed_start = qsStruct.v_start;    %计算起点速度/km/h，停车速度为0
speed_end = qsStruct.v_end;
%% 设置车辆参数
train_mass = trainStruct.mass; %列车质量/t
abc = trainStruct.abc;
rf = trainStruct.rf;
% a_traction_max = 1; %牵引工况时最大的加速度/m/s^2
% a_brake_max = 1;    %制动工况时最大的减速度/m/s^2

%% 处理线路数据数据
Gradient = lineStruct.gradient;           
Curve = lineStruct.curve;
Speed_Limit = lineStruct.speedlimit;        

%% 方向判断
updown = 1;
if position_end < position_start
    updown = -1;
end
%% 最大能力运行
tic
% position_start = Station_Position(num_Station_sta ,2); %计算起点位置公里标/km，取起点车站的中心里程
% position_end = Station_Position(num_Station_end,2); %计算终点位置公里标/km，取终点车站的中心里程
% sum_num_state = round(abs(position_end-position_start)+1); 
x_ = [position_start: position_interval: position_end]';
sum_num_state = length(x_);%状态量的数量

time_start = 0; %计算起点时刻/s
%记录每一步运算过程的状态，
%数据结构：[位置/m，速度/km/h，时刻/s，牵引力/kN，制动力/kN，基本阻力/kN，加速度/m/s^2，时间间隔/s，工况状态，限速/km/h，单位坡度阻力/千分号，单位曲线阻力/s，海拔/m]
State = zeros(sum_num_state,13); 
%工况状态：1-牵引；2-恒速；3-惰行；4-制动
%第13个数据表示控制量，范围是[-1,1];
State(1,2) = speed_start;
State(1,3) = time_start;
%%%%%%%%进行提前赋值可以降低求解时间%%%%%%%%%%%%
%对每个状态的起点里程进行赋值
State(:,1) = x_;
%对每个位置的限速进行赋值
for i = 1:sum_num_state
    State(i,10) = inquire_linedata(State(i,1), Speed_Limit);
end
%终端限速
State(1,10) = speed_start + 1;
State(end,10) = speed_end;
%对每个位置的单位坡度阻力进行赋值
for i = 1:sum_num_state
    State(i,11) = inquire_linedata(State(i,1), Gradient);
end
%对每个位置的单位曲线阻力进行赋值
for i = 1:sum_num_state
    crv = inquire_linedata(State(i,1), Curve);
    if crv ~= 0
        State(i,12) = 600 / abs(crv);
    else
        State(i,12) = 0;
    end
end
%对每个位置的海拔进行赋值
for i = 1:sum_num_state
    State(i,13) = getPosHeight(State(i,1),lineStruct);
end
%%%%%%%%进行提前赋值可以降低求解时间%%%%%%%%%%%%
%与限速进行比较，选取不同的操纵工况
num_state = 1;
while State(num_state,1) < State(end,1)
    f_res = (cal_unit_basicResist(abc, State(num_state,2))+State(num_state,11)+State(num_state,12)) * train_mass * 9.81 / 1000;
    %阻力所产生的减速度/m/s^2,这个加速度的正负需要根据线路具体情况来判断，列车运行相反方向为正
    % a_resistance = 9.81*(State(num_state,11)+State(num_state,12))/1000 + ...
    %     Cal_Resistance(State(num_state,2), trainStruct.mass, trainStruct.abc)/train_mass;
    a_resistance = f_res / train_mass / (1+rf);
    %     State(num_state+1,1) = roundn(State(num_state,1)+position_interval,-3);
    if State(num_state,2) < State(num_state+1,10)-1e-3  %牵引工况 当前速度小于限速 1e-3用于防止截断误差
        %渴望的加速度/m/s^2，可以立即达到高限速2
        a_desired = (1000*1000/3600^2)*(State(num_state+1,10)^2-State(num_state,2)^2)/(2*position_interval);
        %可取的最大加速度/m/s^2，牵引系统能够提供的最大加速度
        a_available = (getMaxFt(State(num_state,2))-f_res)/train_mass/(1+rf);
        %比较三个加速度（渴望的加速度，可取的最大加速度，舒适的加速度），取其中的最小值
        a_utilize = min([a_desired,a_available]);
        %根据被利用的加速度，反算所使用的力/kN
        if a_utilize+a_resistance > 0
            f_t = (a_utilize+a_resistance)*train_mass*(1+rf);  %列车运行方向为正
            f_b = 0;    %列车运行相反方向为正
        else
            f_t = 0;  
            f_b = -(a_utilize+a_resistance)*train_mass*(1+rf);    
        end
        f_r = f_res;
        %计算该运行间隔的运行时间/s
        time_cost = (sqrt((State(num_state,2)/3.6)^2+2*a_utilize*position_interval)-State(num_state,2)/3.6)/(a_utilize);
        %对本牵引过程的状态量进行赋值
        State(num_state,4:8) = [f_t,f_b,f_r,a_utilize,time_cost];
        %对工况状态进行复制
        State(num_state,9) = 1;
        %对下一过程的状态量进行赋值
        State(num_state+1,2) = State(num_state,2) + 3.6*a_utilize*time_cost;
        State(num_state+1,3) = State(num_state,3)+time_cost;
        % State(num_state+1,2:3) = [State(num_state,2)+3.6*a_utilize*time_cost,State(num_state,3)+time_cost];
        %         State(num_state+1,1:3) = [State(num_state+1,1),State(num_state,2)+3.6*a_utilize*time_cost,State(num_state,3)+time_cost];
        %计数量进一位
        num_state = num_state+1;
    elseif abs(State(num_state,2)-State(num_state+1,10)) < 1e-3  %恒速工况
        %被利用的加速度为零/m/s^2，保持列车恒速运行
        a_utilize = 0;
        %先判断牵引能力能不能保持
        if getMaxFt(State(num_state,2)) > f_res
            %牵引力=阻力/kN
            if a_resistance >= 0
                f_t = f_res;
                f_b = 0;
            else
                f_t = 0;
                f_b = -f_res;
            end
        else
            %牵引力无法维持恒速，满级输出
            f_t = getMaxFt(State(num_state,2));
            a_utilize = (getMaxFt(State(num_state,2))-f_res)/train_mass/(1+rf);
        end
        f_r = f_res;
        %计算该运行间隔的运行时间/s
        time_cost = position_interval/(State(num_state,2)/3.6);
        %对本牵引过程的状态量进行赋值
        State(num_state,4:8) = [f_t,f_b,f_r,a_utilize,time_cost];
        %对工况状态进行复制
        State(num_state,9) = 2;
        %对下一过程的状态量进行赋值
        State(num_state+1,2) = State(num_state,2) + 3.6*a_utilize*time_cost;
        State(num_state+1,3) = State(num_state,3)+time_cost;
        % State(num_state+1,2:3) = [State(num_state,2)+3.6*a_utilize*time_cost,State(num_state,3)+time_cost];
        %         State(num_state+1,1:3) = [State(num_state+1,1),State(num_state,2)+3.6*a_utilize*time_cost,State(num_state,3)+time_cost];
        %计数量进一位
        num_state = num_state+1;
    else
        brake_num_state = num_state;    %制动工况开始位置的序号
        State(num_state+1,2) = State(num_state+1,10);   %整个制动过程希望能够达到的速度
        %         State(num_state+1,1:2) = [State(num_state+1,1),State(num_state+1,10)];   %整个制动过程希望能够达到的速度
        while 1
            f_res = (cal_unit_basicResist(abc, State(brake_num_state+1,2))+State(num_state,11)+State(num_state,12)) * train_mass*9.81/1000;
            %阻力所产生的减速度/m/s^2,这个加速度的正负需要根据线路具体情况来判断，列车运行相反方向为正
            % a_resistance = 9.81*(State(num_state,11)+State(num_state,12))/1000+...
            %     Cal_Resistance(State(num_state,2), trainStruct.mass, trainStruct.abc)/train_mass;
            a_resistance = f_res / train_mass / (1+rf);
            %渴望的减速度/m/s^2，可以立即达到低限速  
            a_desired = (1000*1000/3600^2)*(State(brake_num_state,2)^2-State(brake_num_state+1,2)^2)/(2*position_interval);
            %可取的最大减速度/m/s^2，制动系统能够提供的最大减速度
            a_available = getMaxFb(State(brake_num_state+1,2))/train_mass/ (1 + rf) + a_resistance;
            %比较三个减速度（渴望的加速度，可取的最大加速度，舒适的加速度），取合理的值
            a_utilize = min([a_available, a_desired]);
            %根据被利用的减速度，反算所使用的力/kN
            if a_utilize > a_resistance
                f_t = 0;    
                f_b = (a_utilize-a_resistance)*train_mass*(1+rf);  
                
            else
                f_t = -(a_utilize-a_resistance)*train_mass*(1+rf); 
                %防止修正牵引力超出牵引特性边界
                if f_t > getMaxFt(State(num_state+1,2)) 
                    f_t = getMaxFt(State(num_state+1,2));
                end
                a_utilize = (f_t - f_res) / train_mass / (1 + rf); %重算加速度
                f_b = 0;    
            end
            f_r = f_res;
            %计算该运行间隔的运行时间/s
            time_cost = (sqrt((State(brake_num_state+1,2)/3.6)^2+2*a_utilize*position_interval)-State(brake_num_state+1,2)/3.6)/(a_utilize);
            %对本制动过程的状态量进行赋值
            State(brake_num_state,4:8) = [f_t,f_b,f_r,-a_utilize,time_cost];
            %对工况状态进行复制
            State(brake_num_state,9) = 4;
            %利用本次制动后，制动工况开始位置可以达到的速度
            temp_speed = State(brake_num_state+1,2)+3.6*a_utilize*time_cost;
            %判断制动过程是否可以结束
            if temp_speed >= State(brake_num_state,2)-1e-3 
                break;
            else
                State(brake_num_state,2) = temp_speed;
                brake_num_state = brake_num_state-1;
            end
            %如果反算到起始端点发现需要起始就要制动，先跳出循环
            if brake_num_state <= 0
                break;
            end
        end
        %正向计算制动曲线
        if brake_num_state <= 0
            num_state = 1;
            State(num_state,2) = speed_start;
            while State(num_state,2) > State(num_state+1,10) + 1e-3
                num_state = num_state + 1;
                %计算阻力
                f_res = (cal_unit_basicResist(abc, State(num_state-1,2))+State(num_state-1,11)+State(num_state-1,12)) * train_mass * 9.81 / 1000;
                a_resistance = f_res / train_mass / (1+rf);
                %期望减速度，一步到 换到正数了
                a_desired = (1000*1000/3600^2)*(State(num_state-1,2)^2-State(num_state,10)^2)/(2*position_interval);
                %可达减速度，满制 换到正数了
                a_available = getMaxFb(State(num_state-1,2))/train_mass/(1+rf) + a_resistance;
                %实际减速度，选两者的较大值 换到正数了
                a_utilize = min([a_desired a_available]);
                if a_utilize > a_resistance %阻力无法满足制动
                    f_t = 0;
                    f_b = (a_utilize-a_resistance)*train_mass*(1+rf);
                else %阻力就够了，需要少量牵引介入
                    f_t = (a_resistance-a_utilize)*train_mass*(1+rf);
                    f_b = 0;
                end
                f_r = f_res;
                %计算当前状态速度
                State(num_state,2) = sqrt(State(num_state-1,2)^2/3.6^2 + 2*(-a_utilize)*position_interval) * 3.6; 
                %计算该运行间隔的运行时间/s
                % time_cost = (State(num_state,2)-State(num_state-1,2))/3.6 / (-a_utilize);
                time_cost = 2*position_interval / (State(num_state,2)+State(num_state-1,2));
                %对本zhidong过程的状态量进行赋值
                State(num_state,4:8) = [f_t,f_b,f_r,-a_utilize,time_cost];
                %对工况状态进行复制
                State(num_state,9) = 4;
                %对下一过程的状态量进行赋值
                State(num_state,3) = State(num_state-1,3) + time_cost;
            end
            num_state = num_state - 1; %序号对齐
        end
        num_state = num_state+1;
    end
end

%起算点时刻的修正
for num_state = 1:size(State,1)-1
    State(num_state+1,3) = State(num_state,3)+State(num_state,8);
end

% 区间运行时间计算
travel_time_min = Cal_Traveltime(State);
energy_max = Cal_Energy(State)/3600;

% 状态保存
State_max = State;

% figure(1)
% plot(State(:,1),State(:,2));

end

function [ f_r ] = Cal_Resistance(v_kmh, train_mass, abc)
%输入：速度/km/h
%输出：基本阻力/kN

f_r = (abc(1) + abc(2)*v_kmh + abc(3)*v_kmh*v_kmh)*9.81*train_mass/1000;
end

function [ travel_time ] = Cal_Traveltime( State )

%输入：状态/State
%输出：运行时间/s

travel_time = 0;
for num_state = 1:size(State,1)-1
    temp_time = State(num_state,8);
    travel_time = travel_time+temp_time;
end
end

function [ energy ] = Cal_Energy( State )

%输入：状态/State
%输出：能耗/kJ

energy = 0;
for num_state = 1:size(State,1)-1
    temp_energy = 1000*(State(num_state,4));
    energy = energy+temp_energy;
end

end